package org.example.util;

import org.example.bean.Cord;
import org.example.bean.GameMap;
import org.example.bean.JsonGamePos;
import org.example.bean.LazyNode;

import java.util.*;

/**
 * @Desc:
 * @Author: yanghongjun
 * @Date: 2024/5/17
 **/
public class ThetaStarLos {

    private static final double DIAG_COST = 1.41421356237; // sqrt(2), 代表对角线移动的成本

    // 定义八个方向的向量（上，右上，右，右下，下，左下，左，左上）
    private static final List<int[]> DIRECTIONS = Arrays.asList(
            new int[]{0, 1}, new int[]{1, 1}, new int[]{1, 0},
            new int[]{1, -1}, new int[]{0, -1}, new int[]{-1, -1},
            new int[]{-1, 0}, new int[]{-1, 1}
    );


    /**
     * 使用 Theta* 算法在网格地图上寻找从起点到终点的路径。
     *
     * @param start 起点节点
     * @param goal  终点节点
     * @return 从起点到终点的路径节点列表，若找不到路径则返回空列表
     */
    public static List<JsonGamePos> findPath(LazyNode start, LazyNode goal, GameMap gameMap) {
        // 优先级队列，用于存储待处理的节点，按照 fScore 排序
        PriorityQueue<LazyNode> openList = new PriorityQueue<>(Comparator.comparingDouble(LazyNode::getfScore));
        // 已处理节点的集合，用于避免重复处理节点
        Set<String> closedSet = new HashSet<>();

        // 初始化起点的评分和父节点等信息
        start.setgScore(0);
        start.sethScore(heuristic(start, goal));
        start.setfScore(start.getgScore() + start.gethScore());
        // 将起点加入待处理列表
        openList.add(start);

        while (!openList.isEmpty()) {
            // 取出 fScore 最小的节点进行处理
            LazyNode currentNode = openList.poll();
            // 判断当前节点是否为目标节点，若是则返回路径
            if (currentNode.equals(goal)) {
                List<Cord> path = reconstructPath(currentNode);
                // 对路径进行优化，去除不必要的节点，使路径更平滑
                long startTime = System.currentTimeMillis();
                List<JsonGamePos> optimizedPath = gameMap.optimizePath(path);
                System.out.println("ThetaStarLos 路径优化耗时：" + (System.currentTimeMillis() - startTime) +
                        "，优化前节点数：" + path.size() + "，优化后节点数：" + optimizedPath.size());
                return optimizedPath;
            }
            // 将当前节点标记为已处理，并加入已处理节点集合中
            closedSet.add(currentNode.getX() + "," + currentNode.getY());
            // 遍历八个方向，寻找当前节点的邻居节点
            for (int[] direction : DIRECTIONS) {
                int newX = currentNode.getX() + direction[0];
                int newY = currentNode.getY() + direction[1];
                // 判断邻居节点是否在网格范围内且不是障碍物，同时未被处理过
                if (!gameMap.isValid(newX, newY) || closedSet.contains(newX + "," + newY)) {
                    continue;
                }
                // 创建邻居节点对象，并计算其评分等信息
                LazyNode neighbor = new LazyNode(newX, newY);
                /*if (gameMap.canConnectStraight(currentNode.getX(),currentNode.getY(), newX,newY)) {
                    neighbor.setgScore(currentNode.getgScore() + distBetween(currentNode, neighbor));
                } else {
                    neighbor.setgScore(currentNode.getgScore() + 2); // 假设每次移动的成本为 1
                }*/
                neighbor.setgScore(currentNode.getgScore() + distBetween(currentNode, neighbor));

                neighbor.sethScore(heuristic(neighbor, goal));
                neighbor.setfScore(neighbor.getgScore() + neighbor.gethScore());
                neighbor.setParent(currentNode);
                // 若邻居节点不在待处理列表中，或者其 gScore 更小，则将其加入待处理列表
                if (!openList.contains(neighbor) || neighbor.getgScore() < openList.stream()
                        .filter(node -> node.getX() == neighbor.getX() && node.getY() == neighbor.getY())
                        .findFirst()
                        .orElseThrow(() -> new NoSuchElementException("节点在待处理列表中未找到"))
                        .getgScore()) {
                    openList.add(neighbor);
                }
            }
        }
        // 若无法找到路径，则返回空列表
        return Collections.emptyList();
    }


    /**
     * 根据给定的当前节点，重建并返回从起点到该节点的路径。
     *
     * @param currentNode 当前节点
     * @return 从起点到当前节点的路径
     */
    private static List<Cord> reconstructPath(LazyNode currentNode) {
        List<Cord> path = new ArrayList<>();
        while (currentNode != null) {
            path.add(new Cord(currentNode.getX(), currentNode.getY()));
            currentNode = currentNode.getParent();
        }
        Collections.reverse(path);
        return path;
    }

    /**
     * 计算两个节点之间的启发式距离，采用欧几里得距离作为启发式函数。
     *
     * @param a 节点 a
     * @param b 节点 b
     * @return 节点 a 和节点 b 之间的欧几里得距离
     */
    private static double heuristic(LazyNode a, LazyNode b) {
        return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2));
    }

    /**
     * 计算两个相邻节点之间的距离。
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 两点之间的距离
     */
    private static double distBetween(LazyNode nodeA, LazyNode nodeB) {
        double dx = nodeA.getX() - nodeB.getX();
        double dy = nodeA.getY() - nodeB.getY();

        if (dx == 0 || dy == 0) {
            return 1; // Cardinal move
        } else {
            return DIAG_COST; // Diagonal move
        }
    }

    /**
     * 判断给定坐标是否在网格地图范围内且不是障碍物。
     *
     * @param x    横坐标
     * @param y    纵坐标
     * @param grid 网格地图
     * @return 若坐标在网格范围内且不是障碍物则返回 true，否则返回 false
     */
    private static boolean isValid(int x, int y, int[][] grid) {
        return x >= 0 && x < grid.length && y >= 0 && y < grid[0].length && grid[x][y] != 1;
    }
}
